{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Camera Resectioning Example\n",
    "\n",
    "This is a 1:1 transcription of CameraResectioning.cpp, but using custom factors."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "tags": [
     "remove-cell"
    ]
   },
   "source": [
    "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\n",
    "Atlanta, Georgia 30332-0415\n",
    "All Rights Reserved\n",
    "\n",
    "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n",
    "\n",
    "See LICENSE for the license information"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/examples/CameraResectioning.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "tags": [
     "remove-cell"
    ],
    "vscode": {
     "languageId": "markdown"
    }
   },
   "outputs": [],
   "source": [
    "%pip install --quiet gtsam-develop"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from gtsam import Cal3_S2, CustomFactor, LevenbergMarquardtOptimizer, KeyVector\n",
    "from gtsam import NonlinearFactor, NonlinearFactorGraph\n",
    "from gtsam import PinholeCameraCal3_S2, Point2, Point3, Pose3, Rot3, Values\n",
    "from gtsam.noiseModel import Base as SharedNoiseModel, Diagonal\n",
    "from gtsam.symbol_shorthand import X"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def resectioning_factor(\n",
    "    model: SharedNoiseModel,\n",
    "    key: int,\n",
    "    calib: Cal3_S2,\n",
    "    p: Point2,\n",
    "    P: Point3,\n",
    ") -> NonlinearFactor:\n",
    "\n",
    "    def error_func(this: CustomFactor, v: Values, H: list[np.ndarray]) -> np.ndarray:\n",
    "        pose = v.atPose3(this.keys()[0])\n",
    "        camera = PinholeCameraCal3_S2(pose, calib)\n",
    "        if H is None:\n",
    "            return camera.project(P) - p\n",
    "        Dpose = np.zeros((2, 6), order=\"F\")\n",
    "        result = camera.project(P, Dpose) - p\n",
    "        H[0] = Dpose\n",
    "        return result\n",
    "\n",
    "    return CustomFactor(model, KeyVector([key]), error_func)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Assumptions:\n",
    "- Camera: $f = 1$, Image: $100\\times100$, center: $50, 50.0$\n",
    "- Pose (ground truth): $(X_w, -Y_w, -Z_w, [0,0,2.0]^T)$\n",
    "- Known landmarks: $(10,10,0), (-10,10,0), (-10,-10,0), (10,-10,0)$\n",
    "- Perfect measurements: $(55,45), (45,45),  (45,55), (55,55)$\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Final result:\n",
      "\n",
      "Values with 1 values:\n",
      "Value x1: (gtsam::Pose3)\n",
      "R: [\n",
      "\t1, 0, 0;\n",
      "\t0, -1, 0;\n",
      "\t0, 0, -1\n",
      "]\n",
      "t: 0 0 2\n",
      "\n"
     ]
    }
   ],
   "source": [
    "# Create camera intrinsic parameters\n",
    "calibration = Cal3_S2(1, 1, 0, 50, 50)\n",
    "\n",
    "# 1. create graph\n",
    "graph = NonlinearFactorGraph()\n",
    "\n",
    "# 2. add factors to the graph\n",
    "measurement_noise = Diagonal.Sigmas(np.array([0.5, 0.5]))\n",
    "graph.add(\n",
    "    resectioning_factor(\n",
    "        measurement_noise, X(1), calibration, Point2(55, 45), Point3(10, 10, 0)\n",
    "    )\n",
    ")\n",
    "graph.add(\n",
    "    resectioning_factor(\n",
    "        measurement_noise, X(1), calibration, Point2(45, 45), Point3(-10, 10, 0)\n",
    "    )\n",
    ")\n",
    "graph.add(\n",
    "    resectioning_factor(\n",
    "        measurement_noise, X(1), calibration, Point2(45, 55), Point3(-10, -10, 0)\n",
    "    )\n",
    ")\n",
    "graph.add(\n",
    "    resectioning_factor(\n",
    "        measurement_noise, X(1), calibration, Point2(55, 55), Point3(10, -10, 0)\n",
    "    )\n",
    ")\n",
    "\n",
    "# 3. Create an initial estimate for the camera pose\n",
    "initial: Values = Values()\n",
    "initial.insert(X(1), Pose3(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 1)))\n",
    "\n",
    "# 4. Optimize the graph using Levenberg-Marquardt\n",
    "result: Values = LevenbergMarquardtOptimizer(graph, initial).optimize()\n",
    "result.print(\"Final result:\\n\")"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "py312",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
